#ifndef PHIDGET_ALREADY
#define PHIDGET_ALREADY
#include "phidget21.h"
#endif


#define LEFT_FINGER_INDEX 5
#define RIGHT_FINGER_INDEX 6

#define LEFT_FINGER_STRAIGHT_DOWN 153.0
#define RIGHT_FINGER_STRAIGHT_DOWN 22.0

#define GRIPPER_ACCELERATION 1000.0
#define GRIPPER_VELOCITY 32.0


class GripperController {

private:	
	// private functions to operate on both fingers
	void _init(int index) {
		CPhidgetAdvancedServo_setServoType(servo, index, PHIDGET_SERVO_HITEC_HS422);
		CPhidgetAdvancedServo_setAcceleration(servo, index, GRIPPER_ACCELERATION);
		CPhidgetAdvancedServo_setVelocityLimit(servo, index, GRIPPER_VELOCITY);
	}
	void _engage(int index) {
		double pos;
		CPhidgetAdvancedServo_getPosition (servo, index, &pos);
		CPhidgetAdvancedServo_setPosition (servo, index, pos);
		CPhidgetAdvancedServo_setEngaged(servo, index, 1);
	}

public:
	CPhidgetAdvancedServoHandle servo;

	void init(CPhidgetAdvancedServoHandle servoIn) {
		servo = servoIn;
		_init(LEFT_FINGER_INDEX);
		_init(RIGHT_FINGER_INDEX);
	}

	void engage() {
		_engage(LEFT_FINGER_INDEX);
		_engage(RIGHT_FINGER_INDEX);
	}

	// open both fingers by X degrees
	void openByDegrees(double degrees) {
		CPhidgetAdvancedServo_setPosition(servo, LEFT_FINGER_INDEX, LEFT_FINGER_STRAIGHT_DOWN - degrees);
		CPhidgetAdvancedServo_setPosition(servo, RIGHT_FINGER_INDEX, RIGHT_FINGER_STRAIGHT_DOWN + degrees);
	}

	void openByMillimeters(double mms) {
		// TODO: do the math for this
	}

	void disengage() {
		CPhidgetAdvancedServo_setEngaged(servo, LEFT_FINGER_INDEX, 0);
		CPhidgetAdvancedServo_setEngaged(servo, RIGHT_FINGER_INDEX, 0);
	}

};